#include "track_config.hpp"
#include "tv_interface.hpp"
#include "arm.hpp"
#include "task_main.h"
const char trackObjColor[TRACK_COLOR_NONE+1][10] = {"red", "green", "blue", "yellow", "None"};
const char trackObjShape[TRACK_SHAPE_NONE+1][10] = {"rectangle", "triangle", "circle", "None"};
trackResult_t trackResult;
trackResultCollecter trackReceive;
TimerHandle_t runColorTrackTimer;

extern arm Arm;
extern tv_interface TVAPI;
trackConfig_t trackconfig = {
    TRACK_COLOR_RED,        // color
    TRACK_SHAPE_RECTANGLE,  // shape
    10, 100,                // xSizeMin,xSizeMax
    10, 100,                // ySizeMin,ySizeMax
    0, 255,                 // cxStart,cxEnd
    0, 255                  // cyStart,cyEnd
};


/*
    -----颜色跟踪-----
    start
        1.选择视频,开启视频播放
        2.设置识别特征,开启摄像头识别
    ready
        3.发现特征目标暂停视频
    wait
        4.再次识别一次跟踪对象精确定位
    doing
        5.控制云台运动
        6.视频继续播放，摄像头重新开始，回到步骤2
    cplt
        7.播放完成，结束识别
*/
int trackState;
trackConfig_t *curTrackConfig;


void startColorTrack(int video_id, trackConfig_t *config)
{
    if (config == NULL) return;
    if (trackState == 0)
        trackState = 1;
    curTrackConfig = config;
    TVAPI.switchVideo(video_id);
    xTimerStart(runColorTrackTimer, 1);
}

void runColorTrack(TimerHandle_t xTimer)
{
    int temp; 
    switch (trackState)
    {

    case 1:
        if (trackReceive.findResult(curTrackConfig) != -1)
        {
            TVAPI.pauseVideo();
            trackReceive.clearResult();
            trackState = 2;
        }
        break;
    case 2:
        temp = trackReceive.findResult(curTrackConfig);
        if (temp != -1)
        {
            Arm.trackObject(&trackReceive.results[temp]);
            trackState = 3;
        }
        break;
    case 3:
        if (Arm.mode == ARM_IDLE_STATUS)
        {
            trackState = 4;
        }
        break;
    case 4:
        trackState = 0;
        xTimerStop(runColorTrackTimer, portMAX_DELAY);
        break;
    default:

        break;
    }
}

void colorTrackInit(void)
{

    /* 使用动态分配内存的方法创建定时器
	* pcTimerName:定时器名字, 用处不大, 一般在调试时用到
	* xTimerPeriodInTicks: 周期, 以Tick为单位
	* uxAutoReload: 类型, pdTRUE表示自动加载, pdFALSE表示一次性
	* pvTimerID: 回调函数可以使用此参数, 比如分辨是哪个定时器
	* pxCallbackFunction: 回调函数
	* 返回值: 成功则返回TimerHandle_t, 否则返回NULL
	*/	
	runColorTrackTimer = xTimerCreate( "ColorTrackTimer",
								10 / portTICK_PERIOD_MS,
								pdTRUE,
								(void*)1,
								runColorTrack);
}
